#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"

#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h> 
#include <sys/socket.h>
#include <netinet/in.h>
#include <iostream>

using namespace std;


int sockfd;

int newsockfd;

float velocidadTraslacional = 0.0f;

float velocidadRotacional = 0.0f;

void error(const char *msg);

void * communication_thread(void * param);

void socketInit();

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "talker");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

  ros::Rate loop_rate(10);


  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */

cout << "Start!" << endl;
  int count = 0;
  geometry_msgs::Twist base_cmd;
  socketInit();
  pthread_t thread_id;
  int param;
  pthread_create(&thread_id, NULL, communication_thread,(void *)param);
  cout << "Starting ros" << endl;
  while (ros::ok())
  {    	
	cout << "Traslacional:" << velocidadTraslacional << " Rotacional:" << velocidadRotacional << endl;
	base_cmd.linear.x = base_cmd.linear.y = base_cmd.linear.z = 0;	      
	base_cmd.angular.x = base_cmd.angular.y = base_cmd.angular.z = 0;	      
	base_cmd.linear.x = velocidadTraslacional;
	base_cmd.angular.z = velocidadRotacional;
	chatter_pub.publish(base_cmd);
	ros::spinOnce();
	loop_rate.sleep();
	++count;
  }
  
	close(newsockfd);
	close(sockfd);
	return 0;
}


void error(const char *msg)
{
    perror(msg);
    exit(1);
}

void socketInit(){
	int portno;
	socklen_t clilen;	
	struct sockaddr_in serv_addr, cli_addr;
	sockfd = socket(AF_INET, SOCK_STREAM, 0);	
	int val = 1;
	setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,&val,sizeof(val));
	if (sockfd < 0) 
		error("ERROR opening socket");
	bzero((char *) &serv_addr, sizeof(serv_addr));
	portno = 5000;	
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = INADDR_ANY;
	serv_addr.sin_port = htons(portno);
	if (bind(sockfd, (struct sockaddr *) &serv_addr,
		sizeof(serv_addr)) < 0) 
		error("ERROR on binding");
	listen(sockfd,5);
	clilen = sizeof(cli_addr);
	newsockfd = accept(sockfd, 
		(struct sockaddr *) &cli_addr, 
		&clilen);
	if (newsockfd < 0) 
		error("ERROR on accept");	     

}

void * communication_thread(void * param)
{
	const char* p;
	char buffer[256];
	int sockResponse;
	cout << "Running thread" << endl;
	while(1){
		bzero(buffer,256);
		sockResponse = read(newsockfd,buffer,255);
		if (sockResponse < 0) {error("ERROR reading from socket");
			break;}
		if(buffer[0] == 0){
			cout << "Communication ended" << endl;
			break;
		}
		cout << "Received:*" << buffer <<"*"<<  endl;
		p = strtok( buffer, ";" );
		velocidadTraslacional = atof(p);	 
		p = strtok( NULL, ";" );
		velocidadRotacional =atof(p);
	}
	return param;
}
  


